//A lot of this code is adapted from early code of Stephen Ehmann

#include <iostream>
#include <map>
#include <set>
#include <list>
using namespace std;

#include <Glut/glut.h>

#include "TriMesh.h"
#include "TriCirculator.h"
#include "TriReader.h"
#include "TriWriter.h"

#ifdef linux
#include "textures.hpp"
#endif

//static Real INFINITY = 10000000000.0f;

struct LessUndirected
{
   bool operator()(const TriEdge* a, const TriEdge* b) const
   {
      int a1 = min(a->Origin()->Id(), a->Head()->Id());
      int a2 = max(a->Origin()->Id(), a->Head()->Id());
      int b1 = min(b->Origin()->Id(), b->Head()->Id());
      int b2 = max(b->Origin()->Id(), b->Head()->Id());
      return (a1 < b1) || (a1 == b1 && a2 < b2);
   }
};

bool TriMesh::subdivisionLoop = true;


//////////////////////////////////////////////////////////////////////////////
// TriMesh functions
//////////////////////////////////////////////////////////////////////////////

//TriMesh::TriMesh(const Real* vs, const int* fs, int vn, int fn, Real scalefactor)
//{
//   Initialize();
//   Create(vs, fs, vn, fn, scalefactor);
//}

TriMesh::TriMesh(const Real* vs,
                 const int* fs,
                 int vn,
                 int fn,
                 Real scalefactor,
                 const Real* ts,
                 int tn,
                 const int *tis,
                 bool storeUvsInFaces,
                 bool connect)
{
    // vs => Vertices to be read            // vn => Number of vertices
    // fs => Indices for the faces          // fn => Number of faces
    // ts => UV Texture mapping to be read  // tn => Number of texture vertices     // tis => Indices for the face (texture vertices)
    Initialize();
    this->UvsInFaces = storeUvsInFaces;

    if(connect)
    {
      Create(vs, fs, vn, fn, scalefactor, ts, tn, tis);
      quickTriMesh = false;
    }
    else
    {
      quickTriMesh = true;
      CreateWithoutConnect(vs,fs,vn,fn);
    }

    cdf.clear();
}

    
TriMesh::TriMesh(TriMesh* mesh)
{
   Initialize();
   connected=true;
   geomComputed=true;
   useUV = mesh->useUV;
   this->textureMaterialName = mesh->getTextureMaterialName();
   
    this->verts.assign(mesh->Vertices().begin(), mesh->Vertices().end());
    this->faces.assign(mesh->Faces().begin(), mesh->Faces().end());
    this->edges.assign(mesh->Edges().begin(), mesh->Edges().end());

    cdf.clear();
}


TriMesh::~TriMesh(void)
{

   //Delete primitives
   unsigned int i = 0;
   if(quickTriMesh)
   {
      delete [] verts[0];
      delete [] faces[0];
   }
   else
   {
      for (i=0; i < this->verts.size(); i++) {
         delete this->verts[i];
      }

      for (i=0; i < this->faces.size(); i++) {
         delete this->faces[i];
      }
   }
   
   
   verts.clear();
   //cout << "Deleting verts" << endl;
   faces.clear();
   //cout << "Deleting faces" << endl;
   edges.clear();
   //cout << "Deleting edges" << endl;
}

void TriMesh::CalculateCDF( void)
{
   SumDensity = 0;
   for(unsigned i = 0;i < Vertices().size(); i++)
   {
      SumDensity += Vertices()[i]->GetDeformationDensityPowAlpha();
   }
   if(cdf.size() > 0)
      cdf.clear();
   assert(cdf.size() == 0);
   cdf.reserve(Vertices().size());cdf.resize(Vertices().size());
   for(unsigned i = 0;i < Vertices().size(); i++)
   {
      TriVertex* n = Vertices()[i];
      Real value = n->GetDeformationDensityPowAlpha() / this->SumDensity;
      if(i == 0)
         cdf[i] = value;
      else
      {
         cdf[i] = cdf[i-1] + value;
      }
   }
}

int TriMesh::getNodePosition ( double randomValue )
{
   //Inversed Method
   int idNode = -1;
   for(unsigned int k = 0; k < cdf.size();k++)
   {
      if(randomValue < cdf[k])
      {
         if(Vertices()[k]->GetDeformationDensityPowAlpha() != 0.0)
         {
            idNode = k;
            return idNode;
         }
      }
   }
   return idNode;
}

TriMesh* TriMesh::CreatePatchMesh(int numX, int numY, Real size)
{
   //Create vertices
   int vn = (numX + 1) * (numY + 1);
   Real* vs = new Real[3*vn];
   Real x = - 0.5 * (Real)numX * size;
   for (int i=0, k=0; i<=numX; i++, x+=size)
   {
      Real y = - 0.5 * (Real)numY * size;
      for (int j=0; j<=numY; j++, k+=3, y+=size)
      {
         vs[k] = x; vs[k+1] = y; vs[k+2] = 0.0;
         //std::cout << "["<< x << ", " << y << ",0]" << std::endl;
      }
   }

   //Create triangles
   int fn = 2 * numX * numY;
   int* fs = new int[3*fn];
   int voffx = 0;
   for (int i=0, k=0; i<numX; i++, voffx+=(numY+1))
   {
      int voffy = 0;
      for (int j=0; j<numY; j++, k+=6, voffy++)
      {
         int vbase = voffx + voffy;
         //First triangle
         fs[k] = vbase; fs[k+1] = vbase + (numY+1); fs[k+2] = vbase + (numY+2);
         //Second triangle
         fs[k+3] = vbase; fs[k+4] = vbase + (numY+2); fs[k+5] = vbase + 1;
      }
   }

   TriMesh *triMesh = new TriMesh(vs, fs, vn, fn, 1.0f);
   delete[] vs;
   delete[] fs;
   return triMesh;
}

TriMesh* TriMesh::CreateBlockMesh(const Vector3& min, const Vector3& max)
{
   int vn = 8;
   int fn = 12;

   //Set up vertices
   Real vs[24];
   Real *vsp = vs;
   *(vsp++) = min[0]; *(vsp++) = min[1]; *(vsp++) = min[2];
   *(vsp++) = min[0]; *(vsp++) = min[1]; *(vsp++) = max[2];
   *(vsp++) = min[0]; *(vsp++) = max[1]; *(vsp++) = max[2];
   *(vsp++) = min[0]; *(vsp++) = max[1]; *(vsp++) = min[2];
   *(vsp++) = max[0]; *(vsp++) = max[1]; *(vsp++) = min[2];
   *(vsp++) = max[0]; *(vsp++) = max[1]; *(vsp++) = max[2];
   *(vsp++) = max[0]; *(vsp++) = min[1]; *(vsp++) = max[2];
   *(vsp++) = max[0]; *(vsp++) = min[1]; *(vsp++) = min[2];

   //Set up faces
   int fs[36];
   int *fsp = fs;
   *(fsp++) = 0; *(fsp++) = 1; *(fsp++) = 2;
   *(fsp++) = 0; *(fsp++) = 2; *(fsp++) = 3;
   *(fsp++) = 2; *(fsp++) = 5; *(fsp++) = 4;
   *(fsp++) = 2; *(fsp++) = 4; *(fsp++) = 3;
   *(fsp++) = 3; *(fsp++) = 4; *(fsp++) = 7;
   *(fsp++) = 3; *(fsp++) = 7; *(fsp++) = 0;
   *(fsp++) = 0; *(fsp++) = 7; *(fsp++) = 6;
   *(fsp++) = 0; *(fsp++) = 6; *(fsp++) = 1;
   *(fsp++) = 1; *(fsp++) = 6; *(fsp++) = 5;
   *(fsp++) = 1; *(fsp++) = 5; *(fsp++) = 2;
   *(fsp++) = 6; *(fsp++) = 7; *(fsp++) = 4;
   *(fsp++) = 6; *(fsp++) = 4; *(fsp++) = 5;

   TriMesh *triMesh = new TriMesh(vs, fs, vn, fn, 1.0f);
   return triMesh;
}

TriMesh* TriMesh::ReadFromFile(const char *name, FileType type, Real scale)
{
   Real *vs=NULL; //Vertices to be read
   int* fs=NULL; //Vertex indices for the faces
   int vn; //Number of vertices
   int fn; //Number of faces
   Real *ts=NULL;  //Texture vertices (UV) for the texture mapping
   int *tis=NULL; //Texture indices for the texture mapping
   int tn=0; //Number of texture vertices

   bool success = false;
   switch(type)
   {
   case TRI:
      success = TriReader::ReadTriFile(name, vs, fs, vn, fn);
      break;

   case PAR:
      success = TriReader::ReadParFile(name, vs, fs, vn, fn);
      break;

   case OFF:
      success = TriReader::ReadOffFile(name, vs, fs, vn, fn);
      break;

   case OBJ:
      //success = TriReader::ReadObjFile(name, vs, fs, vn, fn);
      success = TriReader::ReadObjFile(name, vs, fs, ts, tis, vn, fn, tn);
      break;

   case OBJTEXFACE:
      //success = TriReader::ReadObjFile(name, vs, fs, vn, fn);
      success = TriReader::ReadObjFileNoUvCorrect(name, vs, fs, ts, tis, vn, fn, tn);
      break;
      
   case DCMESH:
      success = TriReader::ReadDCMeshFile(name, vs, fs, vn, fn);
      break;

   case SMF:
      success = TriReader::ReadSmfFile(name, vs, fs, vn, fn);
      break;

   default:
      break;
   }

   if (!success) return NULL;

   TriMesh *triMesh;
   if (tn==0){
      triMesh = new TriMesh(vs, fs, vn, fn, scale);
   } else {
       if (type == OBJTEXFACE) { //with texture stored in faces
          triMesh = new TriMesh(vs, fs, vn, fn, scale, ts, tn, tis, true);
       } else { //with texture vertices
          triMesh = new TriMesh(vs, fs, vn, fn, scale, ts, tn, tis, false); 
       }
   }
   delete[] vs;
   delete[] fs;
   return triMesh;

}

void TriMesh::ReadFromFile(vector<TriMesh*>& meshes, const char *name, FileType type, Real scale)
{
   Real* vs=NULL; //Vertices to be read
   int* fs=NULL; //Vertex indices for the faces
   int vn; //Number of vertices
   int fn; //Number of faces
    int vert_offset=0;

    meshes.clear();
    ifstream ifile;

    switch(type)
   {
   case OBJ:
        ifile.open(name);
        if (!ifile.is_open()) return;

        TriReader::ParseObjHeader(ifile);

        //Read objects
        while(!ifile.eof())
        {
            if (TriReader::ReadObjFileSingleMesh(ifile, vert_offset, vs, fs, vn, fn)) 
            {
                TriMesh *triMesh = new TriMesh(vs, fs, vn, fn, scale);
                vert_offset += vn;
                
                delete[] vs;
                delete[] fs;
                
                meshes.push_back(triMesh);
            }
        }
        ifile.close();

        break;

   default:
      break;
   }
}


bool TriMesh::ReadFromFile(const char* name, FileType type, Real scale, Real*& vs, int vn)
{

   bool success = false;
   switch(type)
   {
   case TRI:
      success = TriReader::ReadTriFile(name, vs, vn);
      break;

   case OFF:
      success = TriReader::ReadOffFile(name, vs, vn);
      break;

   case OBJTEXFACE:
   case OBJ:
      success = TriReader::ReadObjFile(name, vs, vn);
      break;

   default:
      break;
   }

   // scaling!
   int j=0;
   for (int i=0; i<vn; i++, j+=3) {
      vs[j] *= scale;
      vs[j+1] *= scale;
      vs[j+2] *= scale;
   }

   return success;
}

void TriMesh::PrintToFile(const char *name, FileType type, Real scale, const char *meshName)
{
   //Copy vertices, normals and indices to vectors
   vector<Vector3> pos(verts.size());
   for(int i=0; i<(int)verts.size(); i++)
   {
      pos[i] = verts[i]->Position() * scale;
   }
   vector<Vector3> norm(verts.size());
   for(int i=0; i<(int)verts.size(); i++)
   {
      norm[i] = verts[i]->Normal();
   }
   vector<int> inds(faces.size()*3);
   for(int i=0, j=0; i<(int)faces.size(); i++, j+=3)
   {
      inds[j] = faces[i]->Vertex1()->Id();
      inds[j+1] = faces[i]->Vertex2()->Id();
      inds[j+2] = faces[i]->Vertex3()->Id();
   }

   //Write to file
   switch(type)
   {
   case OBJ:
      TriWriter::WriteObjFile(name, pos, norm, inds, meshName);
      break;
   case TRI:
      TriWriter::WriteTriFile(name, pos, inds);
      break;
   case OFF:
      TriWriter::WriteOFFFile(name, pos, inds);
      break;
   default:
      break;
   }

}

void TriMesh::PrintToFileFacesNotMarked(const char *name, FileType type, Real scale, const char *meshName)
{
   //Copy vertices, normals and indices to vectors
   vector<Vector3> pos(verts.size());
   for(int i=0; i<(int)verts.size(); i++)
   {
      pos[i] = verts[i]->Position() * scale;
   }
   vector<Vector3> norm(verts.size());
   for(int i=0; i<(int)verts.size(); i++)
   {
      norm[i] = verts[i]->Normal();
   }
   vector<int> inds;
   for(int i=0; i<(int)faces.size(); i++)
   {
	   if(faces[i]->Marked())
	   {   
		   continue;
	   }
      inds.push_back(faces[i]->Vertex1()->Id());
      inds.push_back(faces[i]->Vertex2()->Id());
      inds.push_back(faces[i]->Vertex3()->Id());
   }

   //Write to file
   switch(type)
   {
   case OBJ:
      TriWriter::WriteObjFile(name, pos, norm, inds, meshName);
      break;
   case TRI:
      TriWriter::WriteTriFile(name, pos, inds);
      break;
   case OFF:
      TriWriter::WriteOFFFile(name, pos, inds);
      break;
   default:
      break;
   }

}


void TriMesh::PrintToFile(ofstream& ofile, FileType type, Real scale, const char *meshName)
{
   //Copy vertices, normals and indices to vectors
   vector<Vector3> pos(verts.size());
   for(int i=0; i<(int)verts.size(); i++)
   {
      pos[i] = verts[i]->Position() * scale;
   }
   vector<Vector3> norm(verts.size());
   for(int i=0; i<(int)verts.size(); i++)
   {
      norm[i] = verts[i]->Normal();
   }
   vector<int> inds(faces.size()*3);
   for(int i=0, j=0; i<(int)faces.size(); i++, j+=3)
   {
      inds[j] = faces[i]->Vertex1()->Id();
      inds[j+1] = faces[i]->Vertex2()->Id();
      inds[j+2] = faces[i]->Vertex3()->Id();
   }

   //Write to file
   switch(type)
   {
   case OBJ:
      TriWriter::WriteObjFile(ofile, pos, norm, inds, meshName);
      break;
   case POV:
      TriWriter::WritePOVFile(ofile, pos, norm, inds, meshName, texId, GetFaceProperties()->invertNormal);
      break;
   case TRI:
   default:
      TriWriter::WriteTriFile(ofile, pos, inds);
      break;
   }
}

void TriMesh::Initialize(void)
{
   this->UvsInFaces = false;
   connected=false;
   geomComputed=false;
   displayHandle=false;
   useUV = false;
   texId=0;
   color=Vector3(0.5, 0.4, 0.4);
   textureMaterialName ="";

   PreComputePositionsCallback = NULL;
}

void TriMesh::QuickStore(const Real* vs, const int* fs, int vn, int fn)
{
   //Create the vertices
   verts.reserve(vn); verts.resize(vn);
   TriVertex *vertsaux = new TriVertex[vn];
      
   for(int i=0, c=0; i<vn; i++, c+=3)
   {
      vertsaux[i].SetId(i);
      vertsaux[i].SetCoords0(Vector3(vs[c], vs[c+1], vs[c+2]));

      //Inicializate texture vertices
      vertsaux[i].SetTextureCoords(Vector2(0.0f, 0.0f));
   }

   for(int i=0; i<vn; i++)
   {
      verts[i] = &vertsaux[i];
   }

   // Texture coords. If it has texture vertices, set to the vertex:
   //if (tn != 0){ useUV = true; } else { useUV = false; }
   //vector<Vector2> textureVerts; //Temporal var
   //textureVerts.reserve(tn); textureVerts.resize(tn);
   //for(int i=0, c=0; i<tn; i++, c+=2)
   //{ 
   //   textureVerts[i] = Vector2(ts[c], ts[c+1]);
   //}
   
    //Create the faces
    faces.reserve(fn); 
    faces.resize(fn);
    TriFace * face;
    TriFace *faceaux = new TriFace[fn];

    for(unsigned int f=0; f<(unsigned int)fn; f++) {
       faceaux[f].Initialize();
       faceaux[f].SetId(f);
    }
    for(unsigned int f=0; f<(unsigned int)fn; f++) {        
        faces[f] = &faceaux[f];
    }


    //Setting the texture coordinates to the faces.
    //if (this->UvsInFaces == true) {
    //   int tex_index;
    //   if (ts != NULL && tis != NULL && tn > 0) {
    //       for(unsigned int f=0; f<(unsigned int)fn; f++) {
    //           face = faces[f];

    //           //Texture coordinates for the first vertex.
    //           tex_index = tis[f * 3];
    //           face->uv0(ts[tex_index * 2], ts[tex_index * 2 + 1]);

    //           //Texture coordinates for the second vertex.
    //           tex_index = tis[f * 3 + 1];
    //           face->uv1(ts[tex_index * 2], ts[tex_index * 2 + 1]);

    //           //Texture coordinates for the third vertex.
    //           tex_index = tis[f * 3 + 2];
    //           face->uv2(ts[tex_index * 2], ts[tex_index * 2 + 1]);
    //       }
    //   }
    //}

    //Create the edges and set pointers for every face.
    //The elements in the same face will be linked, but the 'twin' pointers
    //will not be set.
    int e = 0;
    //Para cada faceta que tiene el modelo
    TriVertex* vert;
    TriEdge* edge;
    edges.reserve(faces.size()*3); edges.resize(faces.size()*3);
    //printf("vs:%d  fs:%d  verts:%d  faces:%d\n", vn*3, fn*3, verts.size(), faces.size());
    //for(int f=0, texid=0; f<(int)faces.size(); f++)
    for(unsigned int f=0; f < faces.size(); f++) {
        //First edge in this face
        vert=verts[fs[e]];
        edge=faces[f]->Edge1();

        // Texture coords. If it has texture vertices, set to the vertex:
        /*if (tn != 0){
           Vector2 uv(textureVerts[tis[e]]);
           vert->SetTextureCoords(uv);
        }*/

        edge->SetId(e++);
        edges[f*3] = edge;
        vert->SetAdjEdge(edge);
        edge->SetOrigin(vert);

        //Second edge in this face
        vert=verts[fs[e]];
        edge=faces[f]->Edge2();

        // Texture coords. If it has texture vertices, set to the vertex:
        /*if (tn != 0){
           Vector2 uv(textureVerts[tis[e]]);
           vert->SetTextureCoords(uv);
        }*/

        edge->SetId(e++);
        edges[f*3+1] = edge;
        vert->SetAdjEdge(edge);
        edge->SetOrigin(vert);

        //Third edge in this face
        vert=verts[fs[e]];
        edge=faces[f]->Edge3();

        // Texture coords. If it has texture vertices, set to the vertex:
        /*if (tn != 0){
           Vector2 uv(textureVerts[tis[e]]);
           vert->SetTextureCoords(uv);
        }*/

        edge->SetId(e++);
        edges[f*3+2] = edge;
        vert->SetAdjEdge(edge);
        edge->SetOrigin(vert);
    }
}

void TriMesh::Store(const Real* vs, const int* fs, int vn, int fn, Real scalefactor, const Real* ts, int tn, const int *tis)
   //void TriMesh::Store(const Real* vs, const int* fs, int vn, int fn, Real scalefactor)
{
   //Create the vertices
   verts.reserve(vn); verts.resize(vn);
   for(int i=0, c=0; i<vn; i++, c+=3)
   {
      verts[i] = new TriVertex();
      verts[i]->SetId(i);
      verts[i]->SetCoords0(Vector3(vs[c]*scalefactor, vs[c+1]*scalefactor, vs[c+2]*scalefactor));

      //Inicializate texture vertices
      verts[i]->SetTextureCoords(Vector2(0.0f, 0.0f));
   }

   // Texture coords. If it has texture vertices, set to the vertex:
   if (tn != 0){ useUV = true; } else { useUV = false; }
   vector<Vector2> textureVerts; //Temporal var
   textureVerts.reserve(tn); textureVerts.resize(tn);
   for(int i=0, c=0; i<tn; i++, c+=2)
   { 
      textureVerts[i] = Vector2(ts[c], ts[c+1]);
   }

   //Create the faces
   faces.reserve(fn); 
   faces.resize(fn);
   TriFace * face;

   for(unsigned int f=0; f<(unsigned int)fn; f++) {
      face = new TriFace();
      face->Initialize();
      face->SetId(f);
      faces[f] = face;
   }


   //Setting the texture coordinates to the faces.
   if (this->UvsInFaces == true) {
      int tex_index;
      if (ts != NULL && tis != NULL && tn > 0) {
         for(unsigned int f=0; f<(unsigned int)fn; f++) {
            face = faces[f];

            //Texture coordinates for the first vertex.
            tex_index = tis[f * 3];
            face->uv0(ts[tex_index * 2], ts[tex_index * 2 + 1]);

            //Texture coordinates for the second vertex.
            tex_index = tis[f * 3 + 1];
            face->uv1(ts[tex_index * 2], ts[tex_index * 2 + 1]);

            //Texture coordinates for the third vertex.
            tex_index = tis[f * 3 + 2];
            face->uv2(ts[tex_index * 2], ts[tex_index * 2 + 1]);
         }
      }
   }

   //Create the edges and set pointers for every face.
   //The elements in the same face will be linked, but the 'twin' pointers
   //will not be set.
   int e = 0;
   //Para cada faceta que tiene el modelo
   TriVertex* vert;
   TriEdge* edge;
   //printf("vs:%d  fs:%d  verts:%d  faces:%d\n", vn*3, fn*3, verts.size(), faces.size());
   //for(int f=0, texid=0; f<(int)faces.size(); f++)
   for(unsigned int f=0; f < faces.size(); f++) {
      //First edge in this face
      vert=verts[fs[e]];
      edge=faces[f]->Edge1();

      // Texture coords. If it has texture vertices, set to the vertex:
      if (tn != 0){
         Vector2 uv(textureVerts[tis[e]]);
         vert->SetTextureCoords(uv);
      }

      edge->SetId(e++);
      edges.push_back(edge);
      vert->SetAdjEdge(edge);
      edge->SetOrigin(vert);

      //Second edge in this face
      vert=verts[fs[e]];
      edge=faces[f]->Edge2();

      // Texture coords. If it has texture vertices, set to the vertex:
      if (tn != 0){
         Vector2 uv(textureVerts[tis[e]]);
         vert->SetTextureCoords(uv);
      }

      edge->SetId(e++);
      edges.push_back(edge);
      vert->SetAdjEdge(edge);
      edge->SetOrigin(vert);

      //Third edge in this face
      vert=verts[fs[e]];
      edge=faces[f]->Edge3();

      // Texture coords. If it has texture vertices, set to the vertex:
      if (tn != 0){
         Vector2 uv(textureVerts[tis[e]]);
         vert->SetTextureCoords(uv);
      }

      edge->SetId(e++);
      edges.push_back(edge);
      vert->SetAdjEdge(edge);
      edge->SetOrigin(vert);
   }
}

void TriMesh::Append(TriMesh* mesh) 
{
   // reserve storage in vectors
   verts.reserve(verts.size() + mesh->Vertices().size());
   faces.reserve(faces.size() + mesh->Faces().size());
   edges.reserve(edges.size() + mesh->Edges().size());

   // copy data and change ids to the second mesh
   int offset = (int)verts.size();
   for (vector<TriVertex*>::iterator itv = mesh->Vertices().begin(); itv != mesh->Vertices().end(); itv++)
   {
      TriVertex *v = *itv;
      v->SetId(v->Id() + offset);
      verts.push_back(v);        
   }

   offset = (int)faces.size();
   for (vector<TriFace*>::iterator itf = mesh->Faces().begin(); itf != mesh->Faces().end(); itf++)
   {
      TriFace *f = *itf;
      f->SetId(f->Id() + offset);
      faces.push_back(f);        
   }

   offset = (int)edges.size();
   for (vector<TriEdge*>::iterator ite = mesh->Edges().begin(); ite != mesh->Edges().end(); ite++)
   {
      TriEdge *e = *ite;
      e->SetId(e->Id() + offset);
      edges.push_back(e);        
   }
}

void TriMesh::Quicksort(TriEdge** es, int p, int r)
{
   if(p<r)
   {
      // Compute a random element to use as the pivot
      int rn=(int)((Real)(r-p+1) * ((Real)rand()) / (RAND_MAX+1.0)) + p;
      int i=p-1;
      int j=r+1;
      TriEdge* x=es[rn];
      TriEdge* te;
      
      // Swap the random element into the first position
      es[rn]=es[p];
      es[p]=x;
      
      while(true)
      {
         j--;
         while(es[j]->Origin()>x->Origin() ||
            (es[j]->Origin()==x->Origin() && es[j]->Twin()->Origin()>x->Twin()->Origin()))
         {
            j--;
         }
         i++;
         while(es[i]->Origin()<x->Origin() ||
            (es[i]->Origin()==x->Origin() && es[i]->Twin()->Origin()<x->Twin()->Origin()))
         {
            i++;
         }
         if(i<j)
         {
            te=es[i];
            es[i]=es[j];
            es[j]=te;
         }
         else
         {
            break;
         }
      }
      
      Quicksort(es, p, j);
      Quicksort(es, j+1, r);
   }
}

bool TriMesh::Connect(void)
{
   int i, j;
   int numEdges2=2*(int)edges.size();
   
   //Create an array of pointers to edges. Here we will store the existing edges,
   //plus the same edges with opposite directions
   TriEdge **sortEdges=new TriEdge*[numEdges2];
   
   //Store the pointers to the existing edges
   vector<TriEdge*>::iterator edgeIndex;
   for(edgeIndex=edges.begin(), i=0; edgeIndex!=edges.end(); edgeIndex++, i++)
   {
      sortEdges[i]=*edgeIndex;
   }
   
   //Store the pointers to edges in the opposite direction
   int maxEdgeId = (int)edges.size() - 1;
   for(edgeIndex=edges.begin(), j=maxEdgeId+1; edgeIndex!=edges.end(); edgeIndex++, i++, j++)
   {
      //Create edge and assign id
      TriEdge *edge=new TriEdge;
      edge->SetId(j);
      
      //Set pointers
      edge->SetOrigin((*edgeIndex)->Head());
      edge->SetTwin((*edgeIndex));
      (*edgeIndex)->SetTwin(edge);
      
      sortEdges[i]=edge;
   }
   
   //Sort the edges lexicographically
   Quicksort(sortEdges, 0, numEdges2-1);
   
   //Run through the sorted list and connect the mesh.
   //Check for successive edges that have the same end vertices.
   //Three things can happen:
   //   -Two edges with identical end vertices: this means that they're twins
   //   -One single edge with a certain pair of end vertices: it's a boundary edge
   //   -More than two edges: a non-manifold edge
   connected=true;
   int good=0, lone=0, nonmanifold=0, incnormal=0, degenerate=0;
   for(i=0; i<numEdges2; )
   {
      for(; i<numEdges2 && sortEdges[i]->Marked(); i++)
      {
         sortEdges[i]->Unmark();
      }
      
      if(i==numEdges2)
      {
         break;
      }

      j=i+1;
      //Check if the edge goes between a vertex and itself
      if(sortEdges[i]->Origin()==sortEdges[i]->Twin()->Origin())
      {
         sortEdges[i]->Twin()->Mark();
         sortEdges[i]->Twin()->SetTwin(NULL);
         sortEdges[i]->SetTwin(NULL);
         i=j;
         degenerate++;
      }
      //Check if it is a lone edge
      else if(j>=numEdges2 || sortEdges[j]->Twin()==NULL || sortEdges[i]->Origin()!=sortEdges[j]->Origin() ||
         sortEdges[i]->Twin()->Origin()!=sortEdges[j]->Twin()->Origin())
      {
         // Found a lone edge.  It must be on the boundary so snip it.
         sortEdges[i]->Twin()->Mark();
         sortEdges[i]->Twin()->SetTwin(NULL);
         sortEdges[i]->SetTwin(NULL);
         i=j;
         lone++;
      }
      //Check if there are more than two equal edges
      else if(j<numEdges2-1 && sortEdges[j+1]->Origin()==sortEdges[j]->Origin() &&
         sortEdges[j+1]->Twin()->Origin()==sortEdges[j]->Twin()->Origin())
      {
         //Found at least 3 equal edges.
         //Break connectivity of all these edges, making them boundary
         for(j=i+1; j<numEdges2 && sortEdges[i]->Origin()==sortEdges[j]->Origin() &&
            sortEdges[i]->Twin()->Origin()==sortEdges[j]->Twin()->Origin(); j++)
         {
            sortEdges[j]->Twin()->Mark();
            sortEdges[j]->Twin()->SetTwin(NULL);
            sortEdges[j]->SetTwin(NULL);
            nonmanifold++;
         }
         sortEdges[i]->Twin()->Mark();
         sortEdges[i]->Twin()->SetTwin(NULL);
         sortEdges[i]->SetTwin(NULL);
         i=j;
         nonmanifold++;
      }
      //Check if there are two edges but with the same orientation
      else if((sortEdges[i]->Id()>numEdges2/2 && sortEdges[j]->Id()>numEdges2/2) ||
         (sortEdges[i]->Id()<numEdges2/2 && sortEdges[j]->Id()<numEdges2/2))
      {
         sortEdges[i]->Twin()->Mark();
         sortEdges[j]->Twin()->Mark();
         sortEdges[i]->Twin()->SetTwin(NULL);
         sortEdges[j]->Twin()->SetTwin(NULL);
         sortEdges[i]->SetTwin(NULL);
         sortEdges[j]->SetTwin(NULL);
         i+=2;
         incnormal+=2;
      }
      //There are two valid edges
      else
      {
         // Found a pair of edges that are equal
         sortEdges[i]->Twin()->Mark();
         sortEdges[j]->Twin()->Mark();
         sortEdges[i]->Twin()->SetTwin(sortEdges[j]);
         sortEdges[j]->Twin()->SetTwin(sortEdges[i]);
         TriEdge *auxEdge=sortEdges[i]->Twin();
         sortEdges[i]->SetTwin(sortEdges[j]->Twin());
         sortEdges[j]->SetTwin(auxEdge);
         i+=2;
         good+=2;
      }
   }
   
   //Delete the unnecessary edges
   for(i=0; i<numEdges2; i++)
   {
      if(sortEdges[i]->Id()>maxEdgeId)
      {
         delete sortEdges[i];
      }
   }
   delete[] sortEdges;
   
   //If the mesh was not connected, reset the twins
   if(!connected)
   {
      for(int f=0; f<(int)faces.size(); f++)
      {
         faces[f]->Edge1()->SetTwin(NULL);
         faces[f]->Edge2()->SetTwin(NULL);
         faces[f]->Edge3()->SetTwin(NULL);
      }
   }
   
#ifdef _DEBUG
   if(connected)
   {
      VerifyTopology();
   }
#endif //_DEBUG
   
   if(connected && GeomComputed())
   {
      //The geometry was already computed. Recompute it!
      cerr << "Recomputing the geometry" << endl;
      ComputeGeometry();
   }

   cerr << "There are " << NumFaces() << " faces" << endl;
   cerr << "There are " << numEdges2/2 << " edges" << endl;
   cerr << good << " good edges" << endl;
   cerr << lone << " boundary edges" << endl;
   cerr << nonmanifold << " non-manifold edges" << endl;
   cerr << degenerate << " degenerate edges" << endl;
   cerr << incnormal << " edges with inconsistent normals" << endl;

   return connected;
}

void TriMesh::CheckBoundary(void)
{
   //Go over all edges, setting boundary flags
   vector<TriEdge*>::iterator edgeIndex;
   for(edgeIndex=edges.begin(); edgeIndex!=edges.end(); edgeIndex++)
   {
      (*edgeIndex)->CheckBoundary();
   }
}

void TriMesh::CheckDegeneracies(void)
{
   //Go over all edges, setting degeneracy flags
   vector<TriEdge*>::iterator edgeIndex;
   for(edgeIndex=edges.begin(); edgeIndex!=edges.end(); edgeIndex++)
   {
      (*edgeIndex)->CheckDegeneracies();
   }

   //Go over all faces, setting degeneracy flags
   for(int f=0; f<(int)faces.size(); f++)
   {
      faces[f]->CheckDegeneracies();
   }
}

void TriMesh::ComputeGeometry(void)
{
   ComputeEdgeFaceGeometry();
   ComputeEdgeGeometry();
   if(!Connected())
   {
      cerr << "TriMesh not connected. Cannot compute all the geometry" << endl;
   }
   else
   {
      ComputeVertexGeometry();
      ComputeFeatures();
   }
#ifdef _DEBUG
   //VerifyGeometry();
#endif //_DEBUG
   geomComputed=true;
   
   ComputeCenter();
   ComputeRadius();
   ComputeBoundingBox();
   ComputeVolume();
   ComputeArea();
   ComputeInertia();
   
}

//bool TriMesh::Create(const Real* vs, const int* fs, Real scalefactor, int vn, int fn)
//{
//   Store(vs, fs, vn, fn, scalefactor);
//   Connect();
//   ComputeOwners();
//   CheckBoundary();
//   CheckDegeneracies();
//   ComputeGeometry();
//   return connected;
//}

bool TriMesh::Create(const Real* vs, const int* fs, int vn, int fn, Real scalefactor, const Real* ts, int tn, const int *tis)
{
   Store(vs, fs, vn, fn, scalefactor, ts, tn, tis);
   Connect();
   ComputeOwners();
   CheckBoundary();
   CheckDegeneracies();
   ComputeGeometry();
   return connected;
}

bool TriMesh::CreateWithoutConnect(const Real* vs, const int* fs, int vn, int fn)
{
   QuickStore(vs, fs, vn, fn);
   return true;
}


#ifdef _DEBUG
bool TriMesh::VerifyTopology(void)
{
   bool correct=true;
   int i, v, f;
   //cerr << "------------- Beginning Topology Verification -------------" << endl;
   for(i=0, v=0; v<(int)verts.size(); i++, v++)
   {
      // Verify the vertex connectivity
      correct=correct && verts[v]->VerifyTopology(i);
   }
   
   for(i=0, f=0; f<(int)faces.size(); i++, f++)
   {
      // Verify the face connectivity
      if(!faces[f]->VerifyTopology(i))
      {
         correct=false;
      }
      
      // Verify the edge connectivity
      if(!faces[f]->Edge1()->VerifyTopology(i, 1))
      {
         correct=false;
      }
      if(!faces[f]->Edge2()->VerifyTopology(i, 2))
      {
         correct=false;
      }
      if(!faces[f]->Edge3()->VerifyTopology(i, 3))
      {
         correct=false;
      }
   }
   //cerr << "------------- Done Topology Verification -------------" << endl;
   return correct;
}

bool TriMesh::VerifyGeometry(void)
{
   bool correct=true;
   int i, f;
   //cerr << "------------- Beginning Geometry Verification -------------" << endl;
   
   for(i=0, f=0; f<(int)faces.size(); i++, f++)
   {
      // Verify the face geometry
      if(!faces[f]->VerifyGeometry(i))
      {
         correct=false;
      }
      
      // Verify the edge geometry
      if(!faces[f]->Edge1()->VerifyGeometry(i, 1))
      {
         correct=false;
      }
      if(!faces[f]->Edge2()->VerifyGeometry(i, 2))
      {
         correct=false;
      }
      if(!faces[f]->Edge3()->VerifyGeometry(i, 3))
      {
         correct=false;
      }
   }
   //cerr << "------------- Done Verification -------------" << endl;
   return correct;
}
#endif

void TriMesh::ComputeBoundingBox(void)
{
    bb.setEmpty();
    TriVertex *v;
    vector<TriVertex*>::iterator itv;
	
    for (itv = verts.begin(); itv != verts.end(); itv++) {
        v = (*itv);
        bb.extend(v->Position());
    }
}

// All edges should be unmarked when this function is called.  When this
// function terminates, they will be unmarked again.
void TriMesh::ComputeEdgeFaceGeometry(void)
{
   if(Connected())
   {
      //Compute edge geometry only once for each pair of half edges, and share it with the twin.
      for(int f=0; f<(int)faces.size(); f++)
      {
         faces[f]->ComputeGeometryTwins();
      }
   }
   else
   {
      //Compute the geometry for each half separately
      for(int f=0; f<(int)faces.size(); f++)
      {
         faces[f]->ComputeGeometry();
      }
   }
}

void TriMesh::ComputeEdgeGeometry(void)
{
   for (int e=0; e<(int)edges.size(); e++)
   {
      edges[e]->ComputeArea();
   }
}

void TriMesh::ComputeVertexGeometry(void)
{
   if(!Connected())
   {
      cerr << "TriVertex geometry cannot be computed if the mesh is not connected" << endl;
      return;
   }

    TriVertex *v;
    vector<TriVertex*>::iterator itv;
    for (itv = verts.begin(); itv != verts.end(); itv++) {
        v = (*itv);
        v->ComputeValence();
    }   

   ComputeVertexNormals();
   ComputeVertexAreas();
}

void TriMesh::ComputeFaceNormals(void)
{
   //Compute face normals
   for(int f=0; f<(int)faces.size(); f++)
   {
      faces[f]->ComputeNormal();
   }
}

void TriMesh::ComputeVertexNormals(void)
{
   //Reset vertex normals
    TriVertex *v;
    vector<TriVertex*>::iterator itv;
    for (itv = verts.begin(); itv != verts.end(); itv++) {
        v = (*itv);
        v->ResetNormal();
    }   

   //Add contribution of face normals
   for(int f=0; f<(int)faces.size(); f++)
   {
      Vector3 areaNormal = faces[f]->Normal() * faces[f]->getArea();
      faces[f]->Vertex1()->AddToNormal(areaNormal);
      faces[f]->Vertex2()->AddToNormal(areaNormal);
      faces[f]->Vertex3()->AddToNormal(areaNormal);
   }

   //Normalize vertex normals
    for (itv = verts.begin(); itv != verts.end(); itv++) {
        v = (*itv);
        v->NormalizeNormal();
    }   
}

void TriMesh::ComputeVertexAreas(void)
{
   //Reset vertex areas
    TriVertex *v;
    vector<TriVertex*>::iterator itv;
    for (itv = verts.begin(); itv != verts.end(); itv++) {
        v = (*itv);
        v->ResetArea();
    }   

   //Add contribution of face areas
   for(int f=0; f<(int)faces.size(); f++)
   {
      faces[f]->Vertex1()->AddToArea(faces[f]->getArea());
      faces[f]->Vertex2()->AddToArea(faces[f]->getArea());
      faces[f]->Vertex3()->AddToArea(faces[f]->getArea());
   }

   //Normalize vertex areas
    for (itv = verts.begin(); itv != verts.end(); itv++) {
        v = (*itv);
        v->NormalizeArea();
    }   
}

void TriMesh::ComputePositions(void)
{
    // to be careful with this callback function
    if (PreComputePositionsCallback != NULL) 
        (*PreComputePositionsCallback)();

    for (int i=0; i<(int)verts.size(); i++)
    {
        verts[i]->UpdatePosition();
    }
}

void TriMesh::ComputeNormals(void)
{
   ComputeFaceNormals();
   ComputeVertexNormals();
}

void TriMesh::UpdateOldPositions(void)
{
    for (int i=0; i<(int)verts.size(); i++)
    {
        verts[i]->UpdatePositionOld();
    }
}

void TriMesh::ComputeCenter(void)
{
   Real area_x2;
   Real total_area;
   Vector3 areav;
   
   center=Vector3(0.0, 0.0, 0.0);
   total_area = 0.0;
   for(int f=0; f<(int)faces.size(); f++)
   {
      areav = Vector3::crossProd(
         faces[f]->Edge1()->Origin()->Position() - faces[f]->Edge2()->Origin()->Position(),
         faces[f]->Edge1()->Origin()->Position() - faces[f]->Edge3()->Origin()->Position());
      area_x2 = areav.getLength();
      total_area += area_x2;
      center += (faces[f]->Edge1()->Origin()->Position() +
         faces[f]->Edge2()->Origin()->Position() +
         faces[f]->Edge3()->Origin()->Position()) * area_x2;
   }
   center *= 1.0/(3.0 * total_area);
}

void TriMesh::ComputeRadius(void)
{
   Real d;
   radius=0.0;

    TriVertex *v;
    vector<TriVertex*>::iterator itv;
    for (itv = verts.begin(); itv != verts.end(); itv++) {
        v = (*itv);
        d=(Center()-v->Position()).getSquaredLength();
        if(d>radius)
        {
            radius=d;
        }
    }
  
   radius=sqrt(radius);
}

void TriMesh::ComputeVolume(void)
{
   const int XCOORD = 0, YCOORD = 1, ZCOORD = 2;

   //volume integrals
   volume = 0.0;
   for(int i=0; i<(int)faces.size(); i++)
   {
      const Vector3& n=faces[i]->Normal();
      Real w = -faces[i]->Distance();
      Real nx = fabs(n[0]);
      Real ny = fabs(n[1]);
      Real nz = fabs(n[2]);

      int A, B, C;   //alpha, beta, gamma
      if(nx>ny && nx>nz)
      {
         C = XCOORD;
      }
      else
      {
         C = (ny > nz) ? YCOORD : ZCOORD;
      }
      A = (C + 1) % 3;
      B = (A + 1) % 3;
      
      Real P1 =0.0, Pa = 0.0, Pb = 0.0;
      for(int j=0; j<3; j++)
      {
         Real a0 = faces[i]->EdgeP(j)->Origin()->Position()[A];
         Real b0 = faces[i]->EdgeP(j)->Origin()->Position()[B];
         Real a1 = faces[i]->EdgeP(j)->Head()->Position()[A];
         Real b1 = faces[i]->EdgeP(j)->Head()->Position()[B];
         Real a0_2 = a0 * a0;
         Real b0_2 = b0 * b0;
         
         Real C1 = a1 + a0;
         Real Ca = a1*C1 + a0_2;
         Real Cb = b1*(b1 + b0) + b0_2;
         
         Real da = a1 - a0;
         Real db = b1 - b0;
         P1 += db*C1;
         Pa += db*Ca;
         Pb += da*Cb;
      }
      P1 /= 2.0;      Pa /= 6.0;      Pb /= -6.0;
      
      Real k1 = 1 / n[C], k2 = k1 * k1;//, k3 = k2 * k1, k4 = k3 * k1;
      
      //face integrals
      Real Fa = k1 * Pa;
      Real Fb = k1 * Pb;
      Real Fc = -k2 * (n[A]*Pa + n[B]*Pb + w*P1);
      
      //Add to volume
      volume += n[XCOORD] * ((A == XCOORD) ? Fa : ((B == XCOORD) ? Fb : Fc));
      
   }
   
}

void TriMesh::ComputeArea(void)
{
   //area integrals
   area = 0.0;
    TriFace *f;
    vector<TriFace*>::iterator itf;
    for (itf = faces.begin(); itf != faces.end(); itf++) {
        f = (*itf);
        area += f->getArea();
    }
}

#define SQR(x) ((x)*(x))
#define CUBE(x) ((x)*(x)*(x))

void TriMesh::ComputeInertia(void)
{
   if (center.getSquaredLength() > 1.0e-6f*radius)
   {
      //cerr << "The inertia tensor assumes that the object reference frame is at the center of mass" << endl;
      return;
   }

   const int XCOORD = 0, YCOORD = 1, ZCOORD = 2;

   //volume integrals
   Vector3 T1 = Vector3::ZERO, T2 = Vector3::ZERO, TP = Vector3::ZERO;
   for(int i=0; i<(int)faces.size(); i++)
   {
      const Vector3& n=faces[i]->Normal();
      Real w = -faces[i]->Distance();
      Real nx = fabs(n[0]);
      Real ny = fabs(n[1]);
      Real nz = fabs(n[2]);

      int A, B, C;   //alpha, beta, gamma
      if(nx>ny && nx>nz)
      {
         C = XCOORD;
      }
      else
      {
         C = (ny > nz) ? YCOORD : ZCOORD;
      }
      A = (C + 1) % 3;
      B = (A + 1) % 3;
      
      Real P1 = 0.0, Pa = 0.0, Pb = 0.0;
      Real Paa = 0.0, Pab = 0.0, Pbb = 0.0;
      Real Paaa = 0.0, Paab = 0.0, Pabb = 0.0, Pbbb = 0.0;
      
      for(int j=0; j<3; j++)
      {
         Real a0 = faces[i]->EdgeP( j )->Origin()->Position()[A];
         Real b0 = faces[i]->EdgeP( j )->Origin()->Position()[B];
         Real a1 = faces[i]->EdgeP( j )->Head()->Position()[A];
         Real b1 = faces[i]->EdgeP( j )->Head()->Position()[B];
         Real da = a1 - a0;
         Real db = b1 - b0;
         Real a0_2 = a0 * a0, a0_3 = a0_2 * a0, a0_4 = a0_3 * a0;
         Real b0_2 = b0 * b0, b0_3 = b0_2 * b0, b0_4 = b0_3 * b0;
         Real a1_2 = a1 * a1, a1_3 = a1_2 * a1;
         Real b1_2 = b1 * b1, b1_3 = b1_2 * b1;
         
         Real C1 = a1 + a0;
         Real Ca = a1*C1 + a0_2, Caa = a1*Ca + a0_3, Caaa = a1*Caa + a0_4;
         Real Cb = b1*(b1 + b0) + b0_2, Cbb = b1*Cb + b0_3, Cbbb = b1*Cbb + b0_4;
         Real Cab = 3*a1_2 + 2*a1*a0 + a0_2, Kab = a1_2 + 2*a1*a0 + 3*a0_2;
         Real Caab = a0*Cab + 4*a1_3, Kaab = a1*Kab + 4*a0_3;
         Real Cabb = 4*b1_3 + 3*b1_2*b0 + 2*b1*b0_2 + b0_3;
         Real Kabb = b1_3 + 2*b1_2*b0 + 3*b1*b0_2 + 4*b0_3;
         
         P1 += db*C1;
         Pa += db*Ca;
         Paa += db*Caa;
         Paaa += db*Caaa;
         Pb += da*Cb;
         Pbb += da*Cbb;
         Pbbb += da*Cbbb;
         Pab += db*(b1*Cab + b0*Kab);
         Paab += db*(b1*Caab + b0*Kaab);
         Pabb += da*(a1*Cabb + a0*Kabb);
      }
      
      P1 /= 2.0;
      Pa /= 6.0;
      Paa /= 12.0;
      Paaa /= 20.0;
      Pb /= -6.0;
      Pbb /= -12.0;
      Pbbb /= -20.0;
      Pab /= 24.0;
      Paab /= 60.0;
      Pabb /= -60.0;
      
      Real k1 = 1 / n[C], k2 = k1 * k1, k3 = k2 * k1, k4 = k3 * k1;
      
      //Face integrals
      Real Faa = k1 * Paa;
      Real Fbb = k1 * Pbb;
      Real Fcc = k3 * (SQR(n[A])*Paa + 2*n[A]*n[B]*Pab + SQR(n[B])*Pbb
         + w*(2*(n[A]*Pa + n[B]*Pb) + w*P1));
      
      Real Faaa = k1 * Paaa;
      Real Fbbb = k1 * Pbbb;
      Real Fccc = -k4 * (CUBE(n[A])*Paaa + 3*SQR(n[A])*n[B]*Paab
         + 3*n[A]*SQR(n[B])*Pabb + CUBE(n[B])*Pbbb
         + 3*w*(SQR(n[A])*Paa + 2*n[A]*n[B]*Pab + SQR(n[B])*Pbb)
         + w*w*(3*(n[A]*Pa + n[B]*Pb) + w*P1));
      
      Real Faab = k1 * Paab;
      Real Fbbc = -k2 * (n[A]*Pabb + n[B]*Pbbb + w*Pbb);
      Real Fcca = k3 * (SQR(n[A])*Paaa + 2*n[A]*n[B]*Paab + SQR(n[B])*Pabb
         + w*(2*(n[A]*Paa + n[B]*Pab) + w*Pa));
      
      T1[A] += n[A] * Faa;
      T1[B] += n[B] * Fbb;
      T1[C] += n[C] * Fcc;
      T2[A] += n[A] * Faaa;
      T2[B] += n[B] * Fbbb;
      T2[C] += n[C] * Fccc;
      TP[A] += n[A] * Faab;
      TP[B] += n[B] * Fbbc;
      TP[C] += n[C] * Fcca;
   }
   
   T1[XCOORD] /= 2; T1[YCOORD] /= 2; T1[ZCOORD] /= 2;
   T2[XCOORD] /= 3; T2[YCOORD] /= 3; T2[ZCOORD] /= 3;
   TP[XCOORD] /= 2; TP[YCOORD] /= 2; TP[ZCOORD] /= 2;
   
   //compute inertia tensor
   inertia.set(
      T2[YCOORD] + T2[ZCOORD], - TP[XCOORD], - TP[ZCOORD],
      - TP[XCOORD], T2[ZCOORD] + T2[XCOORD], - TP[YCOORD],
      - TP[ZCOORD], - TP[YCOORD], T2[XCOORD] + T2[YCOORD]);
   
   //If the body is not located at the center of mass, the following should be added:
   //inertia *= density;
   //inertia[XCOORD][XCOORD] -= mass * (center[YCOORD]*center[YCOORD] + center[ZCOORD]*center[ZCOORD]);
   //inertia[YCOORD][YCOORD] -= mass * (center[ZCOORD]*center[ZCOORD] + center[XCOORD]*center[XCOORD]);
   //inertia[ZCOORD][ZCOORD] -= mass * (center[XCOORD]*center[XCOORD] + center[YCOORD]*center[YCOORD]);
   //inertia[XCOORD][YCOORD] = inertia[YCOORD][XCOORD] += mass * center[XCOORD] * center[YCOORD];
   //inertia[YCOORD][ZCOORD] = inertia[ZCOORD][YCOORD] += mass * center[YCOORD] * center[ZCOORD];
   //inertia[ZCOORD][XCOORD] = inertia[XCOORD][ZCOORD] += mass * center[ZCOORD] * center[XCOORD];
  
}

void TriMesh::ComputeOwners(void)
{
   // Assign ownership to each vertex and edge.
   // This has to be done even if the mesh cannot be connected.
   // It's necessary, e.g., for eliminating redundant elementary tests in self-collision detection.

   // Loop over all the faces, assigning ownership to their vertices
   // At the end, each vertex is owned by one face
   for (int i=0; i<(int)faces.size(); i++)
   {
      TriFace *f = faces[i];
      f->Vertex1()->SetOwner(f);
      f->Vertex2()->SetOwner(f);
      f->Vertex3()->SetOwner(f);
   }

   // To set ownership, use the set of undirected edges
   set<TriEdge*, LessUndirected> aux;

    TriEdge *e;
    vector<TriEdge*>::iterator ite;
    for (ite = edges.begin(); ite != edges.end(); ite++) {
        e = (*ite);
        pair<set<TriEdge*, LessUndirected>::iterator, bool> auxit;
        if ((auxit = aux.insert(e)).second)
        {
            // Insertion was made, use this edges adjacent face
            e->SetOwner(e->AdjFace());
        }
        else
        {
            // No insertion made, use the adjacent face of the returned edge
            e->SetOwner((*auxit.first)->AdjFace());
        }        
    }
}


Vector3 TriMesh::TranslateToCenter(void)
{
   Vector3 pos = center;

   //Translate mesh
   Transform(-center, Quaternion::IDENTITY);

   //Recompute all geometry
   ComputeGeometry();

   //Report new position of center of mass
   return pos;
}

Vector3 TriMesh::TranslateToBBCenter(void)
{
    Vector3 trans = -(bb.min() + bb.max())*0.5;

   //Translate mesh
   Transform(trans, Quaternion::IDENTITY);

   //Recompute all geometry
   ComputeGeometry();

   return trans;
}

//returns the center used for the scaling
Vector3 TriMesh::Scale(const Matrix33& m)
{
    Vector3 avg(0,0,0);

    //for (int i=0; i<(int)verts.size(); i++) 
    //  avg += verts[i]->PositionInit();
    //avg *= 1.0/(float)verts.size();

    avg = (bb.max() + bb.min())*0.5f;

    
    for (int i=0; i<(int)verts.size(); i++) {
        verts[i]->SetCoords0(m*(verts[i]->PositionInit() - avg) + avg);
    }

    ComputeGeometry();

   return avg;
}

void TriMesh::ScaleWRTPoint(const Matrix33& m, const Vector3& point)
{
    for (int i=0; i<(int)verts.size(); i++) {
        verts[i]->SetCoords0(m*(verts[i]->PositionInit() - point) + point);
    }

    ComputeGeometry();
}



void TriMesh::Translate(const Real x, const Real y, const Real z) {
    Vector3 v(x,y,z);
    for (int i=0; i<(int)verts.size(); i++)
    {
        verts[i]->SetCoords0(verts[i]->PositionInit() + v);
    }
    ComputeGeometry();
}

void TriMesh::Enlarge(Real scale)
{
    for (int i=0; i<(int)verts.size(); i++) {
        verts[i]->SetCoords0((verts[i]->PositionInit() + verts[i]->Normal()*scale));
    }



    ComputeGeometry();
}


void TriMesh::Transform(const Vector3& pos, const Quaternion& rot)
{
   for (int i=0; i<(int)verts.size(); i++)
   {
      verts[i]->SetCoords0(rot * verts[i]->PositionInit() + pos);
   }
    ComputeGeometry();
}

void TriMesh::TransformTemporary(const Vector3& pos, const Quaternion& rot)
{
   for (int i=0; i<(int)verts.size(); i++)
   {
      verts[i]->SetCoords(rot * verts[i]->PositionInit() + pos);
   }
   ComputeGeometry();
}



void TriMesh::Draw(void)
{
#ifdef _USE_OGRE_
    // hola a todos, estoy en ogre


#else   // OpenGL
    bool invertNormals = faceProperties.invertNormal;

    faceProperties.invertNormal = false;
    DrawWithOptions();
    if (invertNormals)
    {
        faceProperties.invertNormal = true;
        DrawWithOptions();
    }
#endif


}

void TriMesh::DrawWithOptions(void)
{
   //Get initial shading and lighting settings
   GLint shading=GL_FLAT;
   glGetIntegerv(GL_SHADE_MODEL, &shading);
   GLboolean lighting;
   glGetBooleanv(GL_LIGHTING, &lighting);
   
   //Set colors and properties
   GLfloat col[4];
   col[0]=faceProperties.invertNormal ? color[0] : color[2];
   col[1]=faceProperties.invertNormal ? color[1] : color[0];
   col[2]=faceProperties.invertNormal ? color[2] : color[1];
   col[3]=1.0; //alpha
   glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, col);
   glMaterialfv(GL_FRONT_AND_BACK, GL_SPECULAR, col);
   glMaterialf(GL_FRONT_AND_BACK, GL_SHININESS, 30.0);
   glColor4fv(col);

   //Textured Mode
#ifdef linux
   if (this->texId != 0) {
      if (this->UvsInFaces == true) {
         //Drawing with textures.
         glEnable(GL_TEXTURE_2D);
         Textures::get_ref().selectTexture(this->texId);
         glColor4f(1.0, 1.0, 1.0, 1.0);

         Vector3 normal;
         glBegin(GL_TRIANGLES);
         for(int f=0; f<(int)faces.size(); f++) {
             normal = faces[f]->Normal();
             //Using the normals depending of the type.
             if(faceProperties.invertNormal) {
                 glNormal3f(-normal[0], -normal[1], -normal[2]);
                 glTexCoord2f(faces[f]->uv1()[0], faces[f]->uv1()[1]);
                 faces[f]->Vertex2()->Draw();
                 glTexCoord2f(faces[f]->uv0()[0], faces[f]->uv0()[1]);
                 faces[f]->Vertex1()->Draw();
                 glTexCoord2f(faces[f]->uv2()[0], faces[f]->uv2()[1]);
                 faces[f]->Vertex3()->Draw();
     
             } else {
                 glNormal3f(normal[0], normal[1], normal[2]);
                 glTexCoord2f(faces[f]->uv0()[0], faces[f]->uv0()[1]);
                 faces[f]->Vertex1()->Draw();
                 glTexCoord2f(faces[f]->uv1()[0], faces[f]->uv1()[1]);
                 faces[f]->Vertex2()->Draw();
                 glTexCoord2f(faces[f]->uv2()[0], faces[f]->uv2()[1]);
                 faces[f]->Vertex3()->Draw();
             }
         }
         glEnd();

         Textures::get_ref().restore_previous_texture();
         glDisable(GL_TEXTURE_2D);

      } else {
          
                     
         glColor4f(1.0, 1.0, 1.0, 1.0);
         Textures::get_ref().selectTexture(this->texId);

         glBegin(GL_TRIANGLES);
         TriFace * a_face;
         Vector2 uvs;
         Vector3 vertex;
         for(int f=0; f<(int)faces.size(); f++) {
            a_face = faces[f];
           
            uvs = a_face->Vertex1()->TextureCoord();
            vertex = a_face->Vertex1()->Position();
            glTexCoord2f(uvs[0], uvs[1]);
            glVertex3f(vertex[0], vertex[1], vertex[2]);


            uvs = a_face->Vertex2()->TextureCoord();
            vertex = a_face->Vertex2()->Position();
            glTexCoord2f(uvs[0], uvs[1]);
            glVertex3f(vertex[0], vertex[1], vertex[2]);

            uvs = a_face->Vertex3()->TextureCoord();
            vertex = a_face->Vertex3()->Position();
            glTexCoord2f(uvs[0], uvs[1]);
            glVertex3f(vertex[0], vertex[1], vertex[2]);
         } 
         glEnd();
      }

      Textures::get_ref().restore_previous_texture();
   } else 
#endif //LINUX
   {
      //No Textured Mode
      //Set properties for rendering faces
      if(shading==GL_SMOOTH && lighting && Connected() && GeomComputed()) {
         faceProperties.vertexNormals=true;
         faceProperties.faceNormal=false;
      } else if(GeomComputed()) {
         faceProperties.vertexNormals=false;
         faceProperties.faceNormal=true;
      } else {
         faceProperties.vertexNormals=false;
         faceProperties.faceNormal=false;
      }
      
      //Render faces
      GLfloat red[]={0.8f, 0.2f, 0.2f, 1.0f};
      GLfloat *oldcolor;
        
      glBegin(GL_TRIANGLES);
      for(int f=0; f<(int)faces.size(); f++) {
           oldcolor = faceProperties.smoothColor;
           // DRAW HANDLE !!!!
           if (displayHandle) {
              if (faces[f]->IsGrasped())
                  faceProperties.smoothColor = red;
           }
           faces[f]->Draw(&faceProperties);
           faceProperties.smoothColor = oldcolor;
      }
      glEnd();


       //// Render normals
       //glBegin(GL_LINES);
       //for (int f=0; f<(int)verts.size(); f++)
       //{
       //  Vector3 center = verts[f]->Position();
       //  Real EPS = 0.05;

       //  glColor3f(0,0,0);
       //  glVertex3f(center[0], center[1], center[2]);
       //  Vector3 normal = verts[f]->Normal();
       //  center += EPS*normal;

       //  glVertex3f(center[0], center[1], center[2]);
       //}
       //glEnd();


      if (faceProperties.edges) {
         //// Render edges
         TriVertex * v1, * v2, *v3;
         glColor4f(0.0f, 0.0f, 0.0f, 1.0f);
         glBegin(GL_LINES);
         for(int f=0; f<(int)faces.size(); f++) {
            v1 = faces[f]->Vertex1();
            v2 = faces[f]->Vertex2();
            v3 = faces[f]->Vertex3();

            glVertex3f(v1->Position()[0], v1->Position()[1], v1->Position()[2]);
            glVertex3f(v2->Position()[0], v2->Position()[1], v2->Position()[2]);

            glVertex3f(v2->Position()[0], v2->Position()[1], v2->Position()[2]);
            glVertex3f(v3->Position()[0], v3->Position()[1], v3->Position()[2]);

            glVertex3f(v1->Position()[0], v1->Position()[1], v1->Position()[2]);
            glVertex3f(v3->Position()[0], v3->Position()[1], v3->Position()[2]);
         }
         glEnd();
      }

   }
 }


//For meshes shaped like a disk, this method provides the set of vertices closer than a threshold from the center
//This was useful for the flamenco demo
void TriMesh::GetInnerRingVertices(Real threshold, vector<TriVertex*>& innerRing)
{
   Real thresholdSq = threshold * threshold;

   for (int i=0; i<(int)verts.size(); i++)
   {
      if ((verts[i]->Position() - Center()).getSquaredLength() < thresholdSq && verts[i]->IsOnBoundary()) 
         innerRing.push_back(verts[i]);
   }
}

vector<TriEdge*>* TriMesh::GetUndirectedEdges(void)
{
   //This should work even for non-manifold, unconnected meshes

   //Create the set
   set<TriEdge*, LessUndirected> aux;

   //Insert edges in the set
    TriEdge *e;
    vector<TriEdge*>::iterator ite;
    for (ite = edges.begin(); ite != edges.end(); ite++) {
        e = (*ite);
        aux.insert(e);
    }

   //Copy to a vector
   vector<TriEdge*> *result = new vector<TriEdge*>;
   result->assign(aux.begin(), aux.end());

#if 0
   //Print edges
   for each (TriEdge *e in *result)
   {
      cout << e->Origin()->Id() << " " << e->Head()->Id() << endl;
   }
#endif

   return result;
}

vector<int>* TriMesh::GetUndirectedEdgesInds(void)
{
   //This should work even for non-manifold, unconnected meshes

   //Create the set
   set<TriEdge*, LessUndirected> aux;

   //Insert edges in the set
    TriEdge *e;
    vector<TriEdge*>::iterator ite;
    for (ite = edges.begin(); ite != edges.end(); ite++) {
        e = (*ite);
        aux.insert(e);
    }

   //Copy to a vector of indices
   vector<int> *result = new vector<int>;
   result->reserve(aux.size());

    for (ite = edges.begin(); ite != edges.end(); ite++) {
        e = (*ite);
        result->push_back(e->Id());
    }

   return result;
}

vector<TriEdge*>* TriMesh::GetSecondLevelEdges(void)
{
   //This should work even for non-manifold, unconnected meshes

   //Create sets of adjacent vertices for all vertices
   set<TriVertex*> *vertAdj = new set<TriVertex*>[verts.size()];

    TriEdge *e;
    vector<TriEdge*>::iterator ite;
    for (ite = edges.begin(); ite != edges.end(); ite++) {
        e = (*ite);
        vertAdj[e->Origin()->Id()].insert(e->Head());
        vertAdj[e->Head()->Id()].insert(e->Origin());
    }

   //Create final set of edges
   set<TriEdge*, LessUndirected> aux;
   
   //Traverse the sets of vertex adjacencies, and insert level-2 edges
   //HACK: The edges need to be created.
   //Create them in pairs, and make them point (next) to each other.
   //If the edge is not inserted, delete it.


   TriVertex *v, *b;
   set<TriVertex*>::iterator itv, itb;

   for (int i=0; i<(int)verts.size(); i++)
   {
        TriVertex *a = verts[i];

        for (itv = vertAdj[i].begin(); itv != vertAdj[i].end(); itv++) {
            v = (*itv);
            for (itb = vertAdj[v->Id()].begin(); itb != vertAdj[v->Id()].end(); itb++) {
                b = (*itb);
            
                if (vertAdj[i].find(b) == vertAdj[i].end() && b != a)
                {
                TriEdge *edgea = new TriEdge();
                edgea->SetOrigin(a);
                TriEdge *edgeb = new TriEdge();
                edgeb->SetOrigin(b);
                edgea->SetNext(edgeb);
                edgeb->SetNext(edgea);
                if(!aux.insert(edgea).second)
                {
                    delete edgea;
                    delete edgeb;
                }
                }
            }
        }
   }
/*
   for (int i=0; i<(int)verts.size(); i++)
   {
      TriVertex *a = verts[i];
      for each (TriVertex *v in vertAdj[i])
      {
         for each (TriVertex *b in vertAdj[v->Id()])
         {
            if (vertAdj[i].find(b) == vertAdj[i].end() && b != a)
            {
               TriEdge *edgea = new TriEdge();
               edgea->SetOrigin(a);
               TriEdge *edgeb = new TriEdge();
               edgeb->SetOrigin(b);
               edgea->SetNext(edgeb);
               edgeb->SetNext(edgea);
               if(!aux.insert(edgea).second)
               {
                  delete edgea;
                  delete edgeb;
               }
            }
         }
      }
   }
*/
   //Copy to a vector
   vector<TriEdge*> *result = new vector<TriEdge*>;
   result->assign(aux.begin(), aux.end());

#if 0
   //Print edges
   for each (TriEdge *e in *result)
   {
      cout << e->Origin()->Id() << " " << e->Head()->Id() << endl;
   }
#endif

   delete[] vertAdj;

   return result;
}

vector<TriEdge*>* TriMesh::GetFlipEdges(void)
{
   //Create sets of adjacent vertices for all vertices
   set<TriVertex*> *vertAdj = new set<TriVertex*>[verts.size()];
    TriEdge *e;
    vector<TriEdge*>::iterator ite;
    for (ite = edges.begin(); ite != edges.end(); ite++) {
        e = (*ite);
        vertAdj[e->Origin()->Id()].insert(e->Head());
        vertAdj[e->Head()->Id()].insert(e->Origin());
    }

   //Create final set of edges
   set<TriEdge*, LessUndirected> aux;

   //Traverse all edges creating flip edges
    for (ite = edges.begin(); ite != edges.end(); ite++) {
        e = (*ite);
        if (e->Twin() != NULL)
        {
            TriVertex *va = e->Next()->Head();
            TriVertex *vb = e->Twin()->Next()->Head();
            if (vertAdj[va->Id()].find(vb) == vertAdj[va->Id()].end())
            {
                TriEdge *edgea = new TriEdge();
                edgea->SetOrigin(va);
                TriEdge *edgeb = new TriEdge();
                edgeb->SetOrigin(vb);
                edgea->SetNext(edgeb);
                edgeb->SetNext(edgea);
    
                if(!aux.insert(edgea).second)
                {
                delete edgea;
                delete edgeb;
                }
            }
        }
   }

   //Copy to a vector
   vector<TriEdge*> *result = new vector<TriEdge*>;
   result->assign(aux.begin(), aux.end());

   delete[] vertAdj;

   return result;

}


void TriMesh::sortFaces(){

   list<TriFace*> newFaces;
   for(unsigned int i=0;i<faces.size();i++){
      newFaces.insert(newFaces.begin(),faces[i]);
   }
   newFaces.sort(compareFace());
   list<TriFace*>::iterator it;
   int i=0;
   for(it=newFaces.begin();it!=newFaces.end();it++,i++){
      faces[i] = (*it);
   }
   vector<TriEdge*> newEdges;
   newEdges.resize(edges.size());
   for(unsigned int i=0;i<faces.size();i++){
      faces[i]->SetId(i);
      newEdges[i*3] = faces[i]->Edge1();
      faces[i]->Edge1()->SetId(i*3);
      newEdges[i*3+1] = faces[i]->Edge2();
      faces[i]->Edge2()->SetId(i*3+1);
      newEdges[i*3+2] = faces[i]->Edge3();
      faces[i]->Edge3()->SetId(i*3+2);
   }
   edges = newEdges;
}



void TriMesh::ComputeFeatures(void)
{
    //Compute feature edges
    bool normalCriterion, axisCriterion;
    TriEdge *e;
    vector<TriEdge*>::iterator ite;
    for (ite = edges.begin(); ite != edges.end(); ite++)
    {
        e = (*ite);

        if (!e->Twin()) continue; //If it's on the boundary, it's not a feature

        normalCriterion = Vector3::dotProd(e->AdjFace()->Normal(), e->Twin()->AdjFace()->Normal()) < 0.4;
        axisCriterion =
            fabs(Vector3::dotProd(e->Direction(), Vector3::UNIT_X)) < 0.01 ||
            fabs(Vector3::dotProd(e->Direction(), Vector3::UNIT_Y)) < 0.01 ||
            fabs(Vector3::dotProd(e->Direction(), Vector3::UNIT_Z)) < 0.01;
        if (normalCriterion && axisCriterion)
        {
            e->SetFeature(true);
            e->Origin()->SetFeature(true);
            e->Head()->SetFeature(true);
            e->Origin()->SetAdjEdge(e); //This will make the disconnection easier
        }
        else
        {
            e->SetFeature(false);
        }
    }

   //Compute corner vertices
    TriVertex *v;
    vector<TriVertex*>::iterator itv;
    for (itv = verts.begin(); itv != verts.end(); itv++)
    {
        v = (*itv);

        if (v->IsOnBoundary())
        {
           //If the vertex is on the boundary, use the angle between boundary edges as corner criterion
           TriEdge * edgeA = NULL;
           TriEdge * edgeB = NULL;
           TriEdge * edge = NULL;
           CWVertexCirculator loop(v);
           for(loop.Start(); !loop.IsDone(); loop.Advance())
           {
              edge=loop.Get();
              if(!edge->Twin()) edgeA = edge;
              if(!edge->Prev()->Twin()) edgeB = edge->Prev();
           }
           bool angleCriterion = true;
           if (edgeA != NULL && edgeB != NULL)
                angleCriterion = Vector3::dotProd(edgeA->Direction(), edgeB->Direction()) < 0.1;
           if (angleCriterion)
           {
              v->SetCorner(true);
           }
        }
        else
        {
           //If the vertex is not on the boundary, set as corner if there are more than two feature edges
           int feat = 0;
           CWVertexCirculator loop(v);
           for(loop.Start(); !loop.IsDone(); loop.Advance())
           {
              TriEdge *edge=loop.Get();
              if(edge->IsFeature())
              {
                 feat++;
              }
           }
           if (feat > 2)
           {
              v->SetCorner(true);
           }
        }
    }
}

void TriMesh::DisconnectAtFeatures(void)
{
    TriEdge *e;
    vector<TriEdge*>::iterator ite;

   //Now go over the mesh, and at feature vertices create new ones and set origins for edges
   int nverts = (int)verts.size(); //Use the number of vertices at the beginning!
   for(int i=0; i<nverts; i++)
   {
      TriVertex *v = verts[i];
      if (!v->IsFeature())
      {
         continue;
      }

      //Note that we already start at a feature edge
      TriVertex *vToAssign = v;
      CCWVertexCirculator loop(v);
      loop.Start();
      TriEdge *edge = loop.Get();
      edge->SetOrigin(vToAssign);
      vToAssign->SetAdjEdge(edge);
      loop.Advance();
      do
      {
         edge = loop.Get();
         if (edge->IsFeature())
         {
            vToAssign = new TriVertex();
            vToAssign->Copy(v);
            vToAssign->SetId((int)verts.size());
            vToAssign->SetAdjEdge(edge);
            verts.push_back(vToAssign);
         }
         edge->SetOrigin(vToAssign);
         loop.Advance();
      } while (!loop.IsDone());
   }

    //Disconnect feature edges
    for (ite = edges.begin(); ite != edges.end(); ite++) {
        e = (*ite);
        if (e->IsFeature())
        {
            e->SetTwin(NULL);
            e->Origin()->SetBoundary();
            e->Head()->SetBoundary();
        }
    }
}

vector< vector<TriVertex*> >* TriMesh::GetClusters(int nClusters) const
{
   //Do clustering based on Lloyd's method

   //Initialize vectors of positions, cluster ids, and cluster positions
   int nverts = (int)Vertices().size();
   vector<Vector3> pos(nverts);
   for (int i=0; i<nverts; i++)
   {
      pos[i] = Vertices()[i]->Position();
   }
   vector<int> id(nverts,-1);
   vector<Vector3> center(nClusters,Vector3::ZERO);
   for (int i=0, j=0; i<nClusters; i++, j+=(nverts/nClusters))
   {
      center[i] = pos[j];
   }

   //Loop until clusters don't change
   bool changed = true;
   while(changed)
   {
      //Compute cluster ids for all vertices
      changed = false;
      for(int i=0; i<nverts; i++)
      {
         //Find closest cluster and assign
         Real ddMin = 1e10;
         int idMin = -1;
         for(int j=0; j<nClusters; j++)
         {
            Real dd = (pos[i] - center[j]).getSquaredLength();
            if (dd < ddMin)
            {
               ddMin = dd;
               idMin = j;
            }
         }
         //Check if the center id for this vertex changed
         if (id[i] != idMin)
         {
            id[i] = idMin;
            changed = true;
         }
      }

      //Compute cluster centers
      for(int i=0; i<nClusters; i++)
      {
         center[i] = Vector3::ZERO;
      }
      vector<int> numVerts(nClusters,0);
      for(int i=0; i<nverts; i++)
      {
         center[id[i]] += pos[i];
         numVerts[id[i]]++;
      }
      for(int i=0; i<nClusters; i++)
      {
         center[i] *= (1.0/(Real)numVerts[i]);
      }
   }

   //Create the sets with vertex ids
   vector< vector<TriVertex*> >* clusters = new vector< vector<TriVertex*> >(nClusters);
   for (int i=0; i<nverts; i++)
   {
      (*clusters)[id[i]].push_back(Vertices()[i]);
   }
   return clusters;
}

vector<TriVertex *>* TriMesh::getTriVertexInBB (const AABB &bb, Real tolerance, bool useX, bool useY, bool useZ) const
{
    // Tolerance is a float value.
    // If useX is false then it's not use X coordenate on a bounding box contain method
    // The same work for useY and useZ
    vector <TriVertex*> *vertexFound = new vector <TriVertex*>;

    // Is more efficient to use dicotomical tree.
    for (unsigned int i = 0; i < this->Vertices().size(); i++)
    {
        // If you need the real position, you must call UpdatePosition(). //this->Vertices()[i]->UpdatePosition()
        //if (bb.containWithTolerance(this->Vertices()[i]->Position(), tolerance, useX, useY,useZ))
        if(bb.contains(this->Vertices()[i]->Position()))
        {
            vertexFound->push_back(this->Vertices()[i]);
        }
    }
    return vertexFound;
}

vector<TriVertex *>* TriMesh::getTriVertexOnTop(Real fract)
{
	ComputeBoundingBox();
   Real h  = fract * (bb.max()[1] - bb.min()[1]);
   Real y0 = bb.max()[1];

	vector <TriVertex*> *vertexFound = new vector <TriVertex*>;
    for (unsigned int i = 0; i < this->Vertices().size(); i++)
    {
      if (y0 - this->Vertices()[i]->Position()[1] < h)
	  {
            vertexFound->push_back(this->Vertices()[i]);
      }
    }
    return vertexFound;
}

vector<TriVertex *>* TriMesh::getTriVertexOnBottom(Real fract)
{
	ComputeBoundingBox();
   Real h  = fract * (bb.max()[1] - bb.min()[1]);
   Real y0 = bb.min()[1];

	vector <TriVertex*> *vertexFound = new vector <TriVertex*>;
    for (unsigned int i = 0; i < this->Vertices().size(); i++)
    {
      if (this->Vertices()[i]->Position()[1] - y0 < h)
	  {
            vertexFound->push_back(this->Vertices()[i]);
      }
    }
    return vertexFound;
}

Real TriMesh::GetAverageEdgeLenght (void) const
{
    Real average=0.0f;

    vector<TriEdge*>::const_iterator edge_it;
    for (edge_it = this->Edges().begin(); edge_it != this->Edges().end(); edge_it++)
    {
        average += (*edge_it)->Length();
    }
   return average / (Real) edges.size();
}

void TriMesh::CopyPositions(TriMesh *other)
{
    int numVertex = (int)this->verts.size();
    const std::vector <TriVertex*> &vOthers = other->Vertices();

    for (int i = 0; i < numVertex; i++)
    {
        TriVertex *vFrom, *vTo;

        vFrom = vOthers[i];
        vTo = this->verts[i];

        vTo->Copy(vFrom);
    }
}

void TriMesh::setUVcoordinateFromOBJ(TriMesh *tmOBJ)
{
    TriVertex *v, *vOBJ;
    vector<TriVertex*>::iterator itv;
    vector<TriVertex*>::iterator itvOBJ;

    if (this->Vertices().size() == tmOBJ->Vertices().size())
    {
        for (itv = verts.begin(), itvOBJ = tmOBJ->Vertices().begin(); itv != verts.end(); itv++, itvOBJ++)
        {
            v = (*itv);
            vOBJ = (*itvOBJ);

            v->SetTextureCoords(vOBJ->TextureCoord());
        }   
    }
}
bool TriMesh::AreEqual (TriMesh *mesh1, TriMesh *mesh2)
{
    bool value = true;

    if (mesh1->Vertices().size() != mesh2->Vertices().size()){
        std::cerr << "Mesh1 and Mesh2 have diferents vertices" << std::endl;
        return false;
    }

    if (mesh1->Faces().size() != mesh2->Faces().size()){
        std::cerr << "Mesh1 and Mesh2 have diferents faces" << std::endl;
        return false;
    }

    if (mesh1->Edges().size() != mesh2->Edges().size()){
        std::cerr << "Mesh1 and Mesh2 have diferents edges" << std::endl;
        return false;
    }

    Real x1, y1, z1, x2, y2, z2;
    int size= (int)mesh1->Vertices().size();
    for (int i=0; i< size; i++)
    {
        x1 =mesh1->Vertices().operator [](i)->Position().operator [](0); y1 =mesh1->Vertices().operator [](i)->Position().operator [](1); z1 =mesh1->Vertices().operator [](i)->Position().operator [](2);
        x2 =mesh2->Vertices().operator [](i)->Position().operator [](0); y2 =mesh2->Vertices().operator [](i)->Position().operator [](1); z2 =mesh2->Vertices().operator [](i)->Position().operator [](2);
        if ( (x1 != x2) || (y1 != y2) || (z1 != z2) )
        {
            value = false;
            std::cerr << "[" << i << "/"<<size<<"] Diferent Position=> Mesh1 (" << x1 <<", " << y1 << ", " << z1 << ") \t\t and Mesh2 (" << x2 <<", " << y2 << ", " << z2 << ")" <<std::endl;
        }
        
        x1 =mesh1->Vertices().operator [](i)->PositionInit().operator [](0); y1 =mesh1->Vertices().operator [](i)->PositionInit().operator [](1); z1 =mesh1->Vertices().operator [](i)->PositionInit().operator [](2);
        x2 =mesh2->Vertices().operator [](i)->PositionInit().operator [](0); y2 =mesh2->Vertices().operator [](i)->PositionInit().operator [](1); z2 =mesh2->Vertices().operator [](i)->PositionInit().operator [](2);
        if ( (x1 != x2) || (y1 != y2) || (z1 != z2) )
        {
            value = false;
            std::cerr << "[" << i << "/"<<size<<"] Diferent PositionInit=> Mesh1 (" << x1 <<", " << y1 << ", " << z1 << ") \t\t and Mesh2 (" << x2 <<", " << y2 << ", " << z2 << ")" <<std::endl;
        }

        x1 =mesh1->Vertices().operator [](i)->PositionOld().operator [](0); y1 =mesh1->Vertices().operator [](i)->PositionOld().operator [](1); z1 =mesh1->Vertices().operator [](i)->PositionOld().operator [](2);
        x2 =mesh2->Vertices().operator [](i)->PositionOld().operator [](0); y2 =mesh2->Vertices().operator [](i)->PositionOld().operator [](1); z2 =mesh2->Vertices().operator [](i)->PositionOld().operator [](2);
        if ( (x1 != x2) || (y1 != y2) || (z1 != z2) )
        {
            value = false;
            std::cerr << "[" << i << "/"<<size<<"] Diferent PositionOld=> Mesh1 (" << x1 <<", " << y1 << ", " << z1 << ") \t\t and Mesh2 (" << x2 <<", " << y2 << ", " << z2 << ")" <<std::endl;
        }
    }// End for

    size= (int)mesh1->Faces().size();
    for (int i=0; i< size; i++)
    {
        if (mesh1->Faces().operator [](i)->Id() != mesh2->Faces().operator [](i)->Id())
        {
            value = false;
            std::cerr << "Mesh1 and Mesh2 have diferent ID faces with num=" << i <<std::endl;
        }
        
        if (
            (mesh1->Faces().operator [](i)->Edge1()->Id() != mesh2->Faces().operator [](i)->Edge1()->Id()) ||
            (mesh1->Faces().operator [](i)->Edge2()->Id() != mesh2->Faces().operator [](i)->Edge2()->Id()) ||
            (mesh1->Faces().operator [](i)->Edge3()->Id() != mesh2->Faces().operator [](i)->Edge3()->Id())
            )
        {
            value = false;
            std::cerr << "Mesh1 and Mesh2 have diferent ID edges in face=" << i <<std::endl;
        }
    }//End for

    size= (int)mesh1->Edges().size();
    for (int i=0; i< size; i++)
    {
        if (mesh1->Edges().operator [](i)->Id() != mesh2->Edges().operator [](i)->Id())
        {
            value = false;
            std::cerr << "Mesh1 and Mesh2 have diferent ID edges with num=" << i <<std::endl;
        }
        
        if (mesh1->Edges().operator [](i)->Direction().getSquaredLength() != mesh2->Edges().operator [](i)->Direction().getSquaredLength())
        {
            value = false;
            std::cerr << "Mesh1 and Mesh2 have diferent direction edges in edge" << i <<std::endl;
        }
    }//End for

    return value;
}

void TriMesh::CalculateRotationMatrix2D(void)
{
   for(int i=0;i<faces.size();++i)
   {
      faces[i]->CalculateRotationMatrix2D();
   }
}


// METODOS PARA SOLO OPENGL
#ifdef linux

bool TriMesh::loadTexture(string filename) 
{
    if (this->texId != 0) {
        Textures::get_ref().unloadTexture(filename);
    }
    this->texId = Textures::get_ref().loadTexture(filename);
    if (this->texId == 0) {
        cerr << "ERROR: TriMesh::loadTexture: Could not load the texture ["<< filename <<"]\n";
        return false;
    }
    //this->textureMaterialName = filename;
    return true;
}


void TriMesh::unloadTexture() {
   if (this->texId != 0) {
        Textures::get_ref().unloadTexture(this->texId);
    }
}
#endif //LINUX
